A Stochastic Optimal Enhancement of Feedback Control for Unicycle Formations

We consider an optimal feedback control approach for multiple nonholonomic vehicles to achieve a distance-based formation with their neighbors using only local observations. Beginning with a non-optimal feedback control for collision-free flocking, each agent determines an additive correction term to its nonoptimal control from an elliptic Hamilton-Jacobi-Bellman equation so that its actions are optimal and robust to uncertainty. In order to avoid offline spatial discretization of the stationary, high-dimensional cost-to-go function, we exploit the stochasticity of the distributed nature of the problem to develop an equivalent estimation problem in a continuous state space using a path integral representation. Consequently, each agent independently computes its optimal feedback control using a discrete-time Unscented Kalman smoother. Our approach is illustrated by a numerical example in which five agent achieve a pentagon with aligned and equal velocities.